#include <rclcpp/rclcpp.hpp>
#include "euler_to_quat.hpp"

// 但是由于计算误差等因素, 计算过程中四元数会逐渐失去规范化特性，因此必须对四元数做规范化处理

void test(){
    double roll = 0.5; //弧度
    double pitch = 2.0; //弧度
    double yaw = 0.785;  //弧度
    GetQuatFromEuler(roll,pitch,yaw);
    GetQuatFromRpy(roll,pitch,yaw);

    Eigen::Quaterniond quat = Euler2QuatXYZ(roll,pitch,yaw);
    std::cout<<"[Euler2QuatXYZ]:"<<std::endl;
    std::cout << "w: " << quat.w() << std::endl;
    std::cout << "x: " << quat.x() << std::endl;
    std::cout << "y: " << quat.y() << std::endl;
    std::cout << "z: " << quat.z() << std::endl;

    Eigen::Quaterniond quat1 = Euler2QuatZYX(roll,pitch,yaw);
    std::cout<<"[Euler2QuatZYX]:"<<std::endl;
    std::cout << "w: " << quat1.w() << std::endl;
    std::cout << "x: " << quat1.x() << std::endl;
    std::cout << "y: " << quat1.y() << std::endl;
    std::cout << "z: " << quat1.z() << std::endl;

    Eigen::Quaterniond quat2 = EulerAngle2QuatXYZ(Rad2Deg(roll),Rad2Deg(pitch),Rad2Deg(yaw)) ;
    std::cout<<"[EulerAngle2QuatXYZ]:"<<std::endl;
    std::cout << "w: " << quat2.w() << std::endl;
    std::cout << "x: " << quat2.x() << std::endl;
    std::cout << "y: " << quat2.y() << std::endl;
    std::cout << "z: " << quat2.z() << std::endl;

    Eigen::Quaterniond quat3 = EulerAngle2QuatZYX(Rad2Deg(roll),Rad2Deg(pitch),Rad2Deg(yaw)) ;
    std::cout<<"[EulerAngle2QuatZYX]:"<<std::endl;
    std::cout << "w: " << quat3.w() << std::endl;
    std::cout << "x: " << quat3.x() << std::endl;
    std::cout << "y: " << quat3.y() << std::endl;
    std::cout << "z: " << quat3.z() << std::endl;
}

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    test();
    rclcpp::shutdown();
    return 0;
}